/*
 * TITLE:    Vtracker.cc
 *
 * PURPOSE:  Encapsulates the Extended Kalman Filter Tracker system
 *           It wraps around the ball_Tracker and robot_tracker classes
 *           It needs to be wrapped in a single class as there are 
 *           between tracking objects interdependencies
 *          
 * WRITTEN BY: Michael Bowling, Brett Browining
 */
/* LICENSE: */

#include "vtracker.h"
#include <string.h>
#include <assert.h>
#include <stdio.h>

/****************************** CONSTANTS ****************************/

#define ISBLUE(c)          ((c) == TEAM_BLUE)
#define CHOOSETEAM(c, b, y)   (ISBLUE(c) ? (b) : (y))

//#define DEBUG

/****************************** CODE *********************************/

VTracker::VTracker(void)
{
#ifdef DEBUG
  fprintf(stderr, "SIMTRACKER:constructor\n");
#endif

  for (int t = 0; t < NUM_TEAMS; t++) {
    for (int i = 0; i < MAX_TEAM_ROBOTS; i++) {
      index2id[t][i] = -1;
      robots[t][i].set_type(ROBOT_TYPE_NONE);
    }
    for (int i = 0; i < MAX_ROBOT_ID; i++)
      id2index[t][i] = -1;
  }

  ball.set_tracker(this);
}

int VTracker::Type(int team, int robot)
{
  if(vconfig.cover_type[team] == VisionConfig::CoverCMDragons){
    return(ROBOT_TYPE(index2id[team][robot]));
  }else{
    return(ROBOT_TYPE_NONE);
  }
}

// set the configuration for the EKBF's 
void VTracker::SetConfig(const VisionConfig &vcfg,
			 bool use_commands_blue,
			 bool use_commands_yellow)
{
#ifdef DEBUG
  fprintf(stderr, "SIMTRACKER:Setting config\n");
#endif

  vconfig = vcfg;

  // clear out indexes and types
  for (int t = 0; t < NUM_TEAMS; t++) {
    for (int i = 0; i < MAX_TEAM_ROBOTS; i++) {
      index2id[t][i] = -1;
      robots[t][i].set_type(ROBOT_TYPE_NONE);
    }
    for (int i = 0; i < MAX_ROBOT_ID; i++)
      id2index[t][i] = -1;
  }

  // set it all
  int id,robot_type;
  for(int team=0; team<NUM_TEAMS; team++){
    for(int i=0; i<MAX_TEAM_ROBOTS; i++){
      if(vcfg.isRobotActive(team,i)){
	id = vcfg.id[team][i];
	index2id[team][i ] = id;
	id2index[team][id] =  i;

	robot_type = Type(team,i); // ok because we just set index2id
	robots[team][i].set_type(robot_type,
	    ((team == TEAM_BLUE) ? use_commands_blue : use_commands_yellow));
#ifdef DEBUG
	fprintf(stderr, "Setting team %i, ind %i, id %i, type %d\n",
		team, n, id, robot_type);
#endif
      }
    }
  }
}

void VTracker::ResetAll(void) 
{
  ball.reset();
  for (int t = 0; t < NUM_TEAMS; t++) {
    for (int i = 0; i < MAX_TEAM_ROBOTS; i++) {
      if (index2id[t][i] >= 0) 
	robots[t][i].reset();
    }
  }
}
  
void VTracker::GetBallData(vball &vb, double dt)
{
  // fill out tracking info
  bcovar = ball.covariances(dt);
  bcovar.CopyData(vb.variances);

  vector2d tmp = ball.position(dt);
  vb.state.x = (float) tmp.x;
  vb.state.y = (float) tmp.y;
  tmp = ball.velocity(dt);
  vb.state.vx = (float) tmp.x;
  vb.state.vy = (float) tmp.y;

  vb.occluded = ball.occluded;
  vb.occluding_team = ball.occluding_team;
  vb.occluding_robot = ball.occluding_robot;
  vb.occluding_offset = vector2f(ball.occluding_offset.x, ball.occluding_offset.y);
}

void VTracker::GetRobotData(vrobot &vr, int team, int indx, double dt)
{
  // Get the state information from the Kalman filter
  vector2d tmp = robots[team][indx].position(dt);
  vr.state.x = (float) tmp.x;
  vr.state.y = (float) tmp.y;
  vr.state.theta = robots[team][indx].direction(dt);
  
  tmp = robots[team][indx].velocity_raw(dt);
  vr.state.vx = (float) tmp.x;
  vr.state.vy = (float) tmp.y;
  vr.state.vtheta = robots[team][indx].angular_velocity(dt);

  vr.state.stuck = robots[team][indx].stuck(dt);
}
